#include "mv_driver.h"

namespace mv_driver
{
MvDriver::MvDriver(ros::NodeHandle node, ros::NodeHandle private_nh)
  : nh_(node), private_nh_(private_nh)
{
    // 定义节点句柄
  image_transport::ImageTransport it(nh_);
  pub = it.advertise("camera_image", 1);

  private_nh.param("auto_exp", auto_exp, true);
  private_nh.param("auto_time", dwExp, 20000);
  private_nh.param("rate", loop_rate_, 40);
  private_nh.param("resize", sizeScale, 1.0);

  int status = init_camera();
  if(status < 0){
    ROS_INFO("Camera init failed");
    return;
  }

  poll_thread_ = boost::shared_ptr<boost::thread>
    (new boost::thread(boost::bind(&MvDriver::capture, this)));
}

MvDriver::~MvDriver()
{
  private_nh_.shutdown();  // 确保线程能正常退出
  MVStopGrab(hCam);
  MVCloseCam(hCam);
  free(stReadBuff.pData);
  free(pDataRGB);
  cout << "Terminate capture thread" << endl;
}

int MvDriver::init_camera()
{
  // EM200C
  int iCams;
  MV_PixelFormatEnums PixelFormat;

  MVGetNumOfCameras(&iCams);

  if (iCams == 0) {
    ROS_INFO("no cameras");
    return -1;
  } else {
    ROS_INFO("found %d cameras", iCams);
  }

  if (MVOpenCamByIndex(0, &hCam) != MVST_SUCCESS) {
    ROS_INFO("can't open camera");
    return -1;
  } else {
    ROS_INFO("camera opened");
  }

  MVGetWidth(hCam, &dwWidth);
  MVGetHeight(hCam, &dwHeight);
  MVGetPixelFormat(hCam, &PixelFormat);

  if(auto_exp){
    MVSetExposureAuto(hCam, ExposureAuto_Continuous);
  }
  else {MVSetExposureTime(hCam, (u_int32_t)dwExp);}

  return 0;
}

void MvDriver::capture()
{
  ROS_INFO("start capture");

  MVStartGrab(hCam);
  stReadBuff.pData = (uint8_t *)malloc(dwWidth * dwHeight);
  pDataRGB = (uint8_t *)malloc(dwWidth * dwHeight * 3);

  mImageRgb.create(dwHeight, dwWidth, CV_8UC3);

  if (sizeScale != 1.0)
  {
    resizeImage.create((int)dwHeight*sizeScale, (int)dwWidth*sizeScale, CV_8UC3);
  }

  int count = 0;
  ros::Rate loop_rate(loop_rate_);

  while(private_nh_.ok()) {
    if (MVRead(hCam, &stReadBuff) == MVST_SUCCESS) {
      MVBayerNearestNeighbor(stReadBuff.pData, pDataRGB, dwWidth, dwHeight,
                             DC1394_COLOR_FILTER_RGGB);

      memcpy(mImageRgb.data, pDataRGB, dwWidth * dwHeight * 3);

      if (sizeScale != 1.0)
      {
        resize(mImageRgb, resizeImage, Size((int)dwWidth*sizeScale, (int)dwHeight*sizeScale));
        msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", resizeImage).toImageMsg();
        pub.publish(msg);

        ROS_INFO("Image size (%d, %d), read the %dth frame successfully!", resizeImage.cols, resizeImage.rows, count++);
      }
      else{
        msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", mImageRgb).toImageMsg();
        pub.publish(msg);

        ROS_INFO("Image size (%d, %d), read the %dth frame successfully!", mImageRgb.cols, mImageRgb.rows, count++);
      }
      
      loop_rate.sleep();
    }
  }
}
} //end of namespace mv_driver
